/*
 * SimpleCDMOTOR Example
 * Stepper Motor Control via CDBUS/CDNET
 * 
 * This example demonstrates how to use the SimpleCDMOTOR library
 * to control a stepper motor over CDBUS/CDNET protocol
 * 
 * Hardware connections:
 * - Enable pin: D8
 * - Direction pin: D9  
 * - Step pin: D10
 * - Microstep pins: D11, D12, D13
 * - Home switch: D2 (with pullup)
 * - Limit switch: D3 (with pullup)
 * 
 * Required hardware:
 * - Arduino board with HardwareSerial
 * - Stepper motor driver (A4988, DRV8825, etc.)
 * - Stepper motor
 * - Limit switches
 */

#include "SimpleCDMOTOR.h"

// Create motor control instance
SimpleCDMOTOR motor;

// Motor configuration
motor_config_t config = {
    .enable_pin = 8,
    .dir_pin = 9,
    .step_pin = 10,
    .ms1_pin = 11,
    .ms2_pin = 12,
    .ms3_pin = 13,
    .home_pin = 2,
    .limit_pin = 3,
    .steps_per_rev = 200,    // 1.8° per step motor
    .max_speed = 1000,       // steps per second
    .max_accel = 5000,       // steps per second²
    .current_limit = 1000,   // mA
    .kp = 2.0,
    .ki = 0.5,
    .kd = 0.1
};

// Command structure for serial control
struct MotorCommand {
    char command;
    int32_t value;
};

void setup() {
    Serial.begin(115200);
    while (!Serial) {
        ; // wait for serial port to connect
    }
    
    Serial.println("SimpleCDMOTOR Example");
    Serial.println("=====================");
    Serial.println();
    Serial.println("Available commands:");
    Serial.println("  p<position> - Move to absolute position");
    Serial.println("  v<velocity> - Set velocity mode");
    Serial.println("  h - Home the motor");
    Serial.println("  e - Enable motor");
    Serial.println("  d - Disable motor");
    Serial.println("  s - Stop motor");
    Serial.println("  r - Read current position");
    Serial.println("  ? - Show status");
    Serial.println();
    
    // Initialize motor with Serial1 (or change to Serial for software serial)
    motor.begin(&Serial, 0x01, 0x1001);
    
    // Configure motor parameters
    motor.configure(config);
    
    Serial.println("Motor initialized. Waiting for commands...");
}

void loop() {
    motor.update();
    
    // Handle serial commands
    if (Serial.available() > 0) {
        String input = Serial.readStringUntil('\n');
        input.trim();
        
        if (input.length() > 0) {
            processCommand(input);
        }
    }
    
    // Print status every 500ms
    static uint32_t last_status = 0;
    if (millis() - last_status > 500) {
        printStatus();
        last_status = millis();
    }
}

void processCommand(String& input) {
    char command = input.charAt(0);
    String value_str = input.substring(1);
    int32_t value = value_str.toInt();
    
    switch (command) {
        case 'p':
        case 'P':
            if (value_str.length() > 0) {
                motor.setPosition(value);
                Serial.print("Moving to position: ");
                Serial.println(value);
            } else {
                Serial.println("Usage: p<position>");
            }
            break;
            
        case 'v':
        case 'V':
            if (value_str.length() > 0) {
                motor.setVelocity(value);
                Serial.print("Setting velocity: ");
                Serial.println(value);
            } else {
                Serial.println("Usage: v<velocity>");
            }
            break;
            
        case 'h':
        case 'H':
            Serial.println("Homing motor...");
            motor.home();
            break;
            
        case 'e':
        case 'E':
            Serial.println("Enabling motor...");
            motor.enable();
            break;
            
        case 'd':
        case 'D':
            Serial.println("Disabling motor...");
            motor.disable();
            break;
            
        case 's':
        case 'S':
            Serial.println("Stopping motor...");
            motor.stop();
            break;
            
        case 'r':
        case 'R':
            Serial.print("Current position: ");
            Serial.println(motor.getPosition());
            break;
            
        case '?':
            printStatus();
            break;
            
        default:
            Serial.println("Unknown command. Type ? for help.");
            break;
    }
}

void printStatus() {
    Serial.println("--- Motor Status ---");
    Serial.print("Position: ");
    Serial.print(motor.getPosition());
    Serial.print(" -> ");
    Serial.println(motor.getTargetPosition());
    
    Serial.print("State: ");
    switch (motor.getState()) {
        case MOTOR_STATE_DISABLED: Serial.println("DISABLED"); break;
        case MOTOR_STATE_ENABLED:  Serial.println("ENABLED"); break;
        case MOTOR_STATE_MOVING:   Serial.println("MOVING"); break;
        case MOTOR_STATE_HOMING:   Serial.println("HOMING"); break;
    }
    
    Serial.print("Homed: ");
    Serial.println(motor.isHomed() ? "Yes" : "No");
    
    Serial.print("Moving: ");
    Serial.println(motor.isMoving() ? "Yes" : "No");
    
    Serial.println("------------------");
}